{
  "nbformat_minor": 0,
  "nbformat": 4,
  "metadata": {
    "kernelspec": {
      "display_name": "Python 3",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "pygments_lexer": "ipython3",
      "nbconvert_exporter": "python",
      "codemirror_mode": {
        "version": 3,
        "name": "ipython"
      },
      "mimetype": "text/x-python",
      "version": "3.5.2",
      "name": "python",
      "file_extension": ".py"
    }
  },
  "cells": [
    {
      "cell_type": "code",
      "execution_count": null,
      "outputs": [],
      "source": [
        "%matplotlib inline"
      ],
      "metadata": {
        "collapsed": false
      }
    },
    {
      "cell_type": "markdown",
      "source": [
        "\n********************************************************************************\n2D Robot Localization on Real Data\n********************************************************************************\nGoals of this script:\n\n- apply the UKF for the 2D robot localization example on real data.\n\n*We assume the reader is already familiar with the considered problem described\nin the tutorial.*\n\nWe address the same problem as described in the tutorial on our own data.\n\n"
      ],
      "metadata": {}
    },
    {
      "cell_type": "markdown",
      "source": [
        "Import\n==============================================================================\n\n"
      ],
      "metadata": {}
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "outputs": [],
      "source": [
        "from ukfm import LOCALIZATION as MODEL\nimport ukfm\nimport numpy as np\nimport matplotlib\nukfm.utils.set_matplotlib_config()"
      ],
      "metadata": {
        "collapsed": false
      }
    },
    {
      "cell_type": "markdown",
      "source": [
        "Model and Data\n==============================================================================\nThis script uses the :meth:`~ukfm.LOCALIZATION` model.\n\nInstead of creating data, we load recorded data. We have recorded five\nsequences (sequence 2 and 3 are the more interesting).\n\n"
      ],
      "metadata": {}
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "outputs": [],
      "source": [
        "# sequence number\nn_sequence = 3\n# GPS frequency (Hz)\ngps_freq = 2\n# GPS noise standard deviation (m)\ngps_std = 0.1\n# load data\nstates, omegas, ys, one_hot_ys, t = MODEL.load(n_sequence, gps_freq, gps_std)"
      ],
      "metadata": {
        "collapsed": false
      }
    },
    {
      "cell_type": "markdown",
      "source": [
        "Data has been obtained in an experiment conducted at the Centre for Robotics,\nMINES ParisTech. We used a so-called Wifibot, which is a small wheeled robot\nequipped with independent odometers on the left and right wheels, see figure.\nA set of seven highly precise cameras, the OptiTrack motion capture system,\nprovide the reference trajectory (ground truth) with sub-millimeter precision\nat a rate of 120 Hz.\n\n.. figure:: ../images/robot.jpg\n   :scale: 20 %\n   :alt: robot picture.\n   :align: center\n\n   Testing arena with Wifibot robot in the foreground of the picture. We can\n   also see two of the seven Optitrack cameras in the background.\n\n"
      ],
      "metadata": {}
    },
    {
      "cell_type": "markdown",
      "source": [
        "We define noise odometry standard deviation for the filter.\n\n"
      ],
      "metadata": {}
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "outputs": [],
      "source": [
        "odo_std = np.array([0.15,   # longitudinal speed\n                    0.05,   # transverse shift speed\n                    0.15])  # differential odometry"
      ],
      "metadata": {
        "collapsed": false
      }
    },
    {
      "cell_type": "markdown",
      "source": [
        "Filter Design\n==============================================================================\nWe embed here the state in $SE(2)$ with left multiplication, i.e. \n\n - the retraction $\\varphi(.,.)$ is the $SE(2)$ exponential, where\n   the state multiplies on the left the uncertainty $\\boldsymbol{\\xi}$.\n\n- the inverse retraction $\\varphi^{-1}_.(.)$ is the $SE(2)$\n  logarithm.\n\nWe define the filter parameters based on the model parameters.\n\n"
      ],
      "metadata": {}
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "outputs": [],
      "source": [
        "# propagation noise covariance matrix\nQ = np.diag(odo_std ** 2)\n# measurement noise covariance matrix\nR = gps_std ** 2 * np.eye(2)\n# sigma point parameters\nalpha = np.array([1e-3, 1e-3, 1e-3])"
      ],
      "metadata": {
        "collapsed": false
      }
    },
    {
      "cell_type": "markdown",
      "source": [
        "Filter Initialization\n------------------------------------------------------------------------------\nWe initialize the filter with the true state plus an initial heading error of\n30\u00b0, and set corresponding initial covariance matrices.\n\n"
      ],
      "metadata": {}
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "outputs": [],
      "source": [
        "# \"add\" orientation error to the initial state\nSO2 = ukfm.SO2\nstate0 = MODEL.STATE(Rot=states[0].Rot.dot(SO2.exp(30/180*np.pi)),\n                     p=states[0].p)\n# initial state uncertainty covariance matrix\nP0 = np.zeros((3, 3))\n# The state is not perfectly initialized\nP0[0, 0] = (30/180*np.pi)**2"
      ],
      "metadata": {
        "collapsed": false
      }
    },
    {
      "cell_type": "markdown",
      "source": [
        "We define the filter as an instance of the ``UKF`` class.\n\n"
      ],
      "metadata": {}
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "outputs": [],
      "source": [
        "ukf = ukfm.UKF(state0=state0,               #\u00a0initial state\n               P0=P0,                       # initial covariance\n               f=MODEL.f,                   # propagation model\n               h=MODEL.h,                   # observation model\n               Q=Q,                         # process noise covariance\n               R=R,                         # observation noise covariance\n               phi=MODEL.left_phi,          # retraction function\n               phi_inv=MODEL.left_phi_inv,  # inverse retraction function\n               alpha=alpha                  # sigma point parameters\n               )"
      ],
      "metadata": {
        "collapsed": false
      }
    },
    {
      "cell_type": "markdown",
      "source": [
        "Before launching the filter, we set a list for recording estimates along the\nfull trajectory and a 3D array to record covariance estimates.\n\n"
      ],
      "metadata": {}
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "outputs": [],
      "source": [
        "N = t.shape[0]\nukf_states = [ukf.state]\nukf_Ps = np.zeros((N, 3, 3))\nukf_Ps[0] = ukf.P"
      ],
      "metadata": {
        "collapsed": false
      }
    },
    {
      "cell_type": "markdown",
      "source": [
        "Filtering\n------------------------------------------------------------------------------\nThe UKF proceeds as a standard Kalman filter with a for loop.\n\n"
      ],
      "metadata": {}
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "outputs": [],
      "source": [
        "# measurement iteration number (first measurement is for n == 0)\nk = 1\nfor n in range(1, N):\n    # propagation\n    dt = t[n] - t[n-1]\n    ukf.propagation(omegas[n-1], dt)\n    # update only if a measurement is received\n    if one_hot_ys[n] == 1:\n        ukf.update(ys[k])\n        k += 1\n    # save estimates\n    ukf_states.append(ukf.state)\n    ukf_Ps[n] = ukf.P"
      ],
      "metadata": {
        "collapsed": false
      }
    },
    {
      "cell_type": "markdown",
      "source": [
        "Results\n==============================================================================\nWe plot the trajectory, the measurements and the estimated trajectory. We then\nplot the position and orientation error with 95% ($3\\sigma$) confident\ninterval.\n\n"
      ],
      "metadata": {}
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "outputs": [],
      "source": [
        "MODEL.plot_wifibot(ukf_states, ukf_Ps, states, ys, t)"
      ],
      "metadata": {
        "collapsed": false
      }
    },
    {
      "cell_type": "markdown",
      "source": [
        "All results are coherent. This is convincing as the initial heading error is\nrelatively high.\n\n"
      ],
      "metadata": {}
    },
    {
      "cell_type": "markdown",
      "source": [
        "Conclusion\n==============================================================================\nThis script applies the UKF for localizing a robot on real data. The filter\nworks well on this localization problem on real data, with moderate\ninitial heading error.\n\nYou can now:\n\n* test the UKF on different sequences.\n\n* address the UKF for the same problem with range and bearing measurements of\n  known landmarks.\n\n"
      ],
      "metadata": {}
    }
  ]
}